#include <iostream>
#include "backendshen/vertexshen.h"
#include "backendshen/edgeshen.h"
#include "backendshen/problemshen.h"
#include <random>

using namespace myslam::backend;

class CurveFittingVertex:public Vertex
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    CurveFittingVertex():Vertex(3){}
    virtual std::string TypeInfo() const { return "abc"; }
};

class CurveFittingEdge:public Edge
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    CurveFittingEdge(double x,double y):Edge(1,1,std::vector<std::string>{"abc"})
    {
        x_ = x;
        y_ = y;
    }
    virtual void ComputeResidual() override{
        Vec3 abc = verticies_[0]->Parameters();
        //residual_[0] = std::exp(abc(0) * x_ * x_ + abc(1) * x_ + abc(2)) - y_;
        residual_[0] = (abc(0) * x_ * x_ + abc(1) * x_ + abc(2)) - y_;
    }
    void ComputeJacobians() override
    {
        Vec3 abc = verticies_[0]->Parameters();
        //double exp_y = std::exp(abc(0) * x_ * x_ + abc(1) * x_ + abc(2));
        Eigen::Matrix<double,1, 3> jaco_abc;  //误差为1维，状态量为3维  所以是1x3
        //jaco_abc << exp_y * (x_ * x_), exp_y * x_, exp_y;
        jaco_abc << x_ * x_, x_, 1;
        jacobins_[0] = jaco_abc;
    }
    //返回边的类型信息
    std::string TypeInfo()const override 
    {
        return "CurveFittingEdge";
    }

private:
    double x_, y_;
};

int main(int argc, char *argv[])
{
    std::cout<<"开始程序"<<std::endl;
    double a = 10.0, b = 3.0, c = 3.0;
    int N = 1000;
    double w_sigma = 1.;  //噪声Sigma值
    std::default_random_engine generator;
    std::normal_distribution<double> noise(0., w_sigma);
    //构建problem
    Problem problem(Problem::ProblemType::GENERIC_PROBLEM);
    //shared_ptr<CurveFittingVertex> vertex(new CurveFittingVertex());
    shared_ptr<CurveFittingVertex> vertex = std::make_shared<CurveFittingVertex>();
    vertex->SetParameters(Eigen::Vector3d(0., 0., 0.));
    problem.AddVertex(vertex);
    for (int i = 0; i < N;++i)
    {
        double x = i / 10.;
        double n = noise(generator);
        //观测
        //double y = std::exp(a * x * x + b * x + c) + n;
        double y = a * x * x + b * x + c + n;
        std::cout << x << "   " << y << "   " << i << std::endl;
        // 每个观测对应的残差函数
        std::shared_ptr<CurveFittingEdge> edge = std::make_shared<CurveFittingEdge>(x, y);
        std::vector<std::shared_ptr<Vertex>> edge_vertex;
        edge_vertex.push_back(vertex);
        edge->SetVertex(edge_vertex);
        //把这个残差添加到最小二乘问题
        problem.AddEdge(edge);
    }
    std::cout << "\nTest CurveFitting start ..." << std::endl;
    //使用LM求解5
    problem.Solve(30);
    std::cout << "------------After optimization ,we got these parameters:" << std::endl;
    std::cout << vertex->Parameters().transpose() << std::endl;
    std::cout << "--------------groud truth-----------------\n";
    std::cout << a << "    " << b << "     " << c << "       " << std::endl;
}
